/*
	TOPCAT Payload Software, for use in the UKube-1 CubeSat mission.
	
	http://www.bath.ac.uk/elec-eng/invert/topcat.html
	
	Written by:
	Mark Jessop, University of Adelaide
	Talini Pinto Jayawardena, University of Bath
	
	Uses extracts from source code provided by Clyde Space for I2C communications.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    For a full copy of the GNU General Public License, 
    see <http://www.gnu.org/licenses/>.
*/

//////////////////////////////////
// I2C Platform Comms Code
//////////////////////////////////
//
// Created by        : Mark Jessop, 2011-07-28
//
// Based off code provided by Clyde Space for the UKube-1 Payload Emulator

#include <twi_long.h>

void I2C_begin()
{
  twi_setAddress(PAY_ADDR);
  twi_attachSlaveTxEvent(I2C_onRequestService);
  twi_attachSlaveRxEvent(I2C_onReceiveService);

  twi_init();
}

void I2C_onReceiveService(uint8_t* inBytes, int numBytes)
{
  // copy twi rx buffer into local read buffer
  // this enables new reads to happen in parallel
  for(uint8_t i = 0; i < numBytes; ++i){
    i2c_rxd_buffer[i] = inBytes[i];    
  }

  // alert user program
  bitSet(I2C_FLAGS,I2C_NEW_PACKET);
  delayMicroseconds(10);

}

void I2C_onRequestService(void)
{
	if(bitRead(I2C_FLAGS,I2C_TX_PACKET_READY)){
		twi_transmit(i2c_txd_buffer, i2c_tx_length);
		delayMicroseconds(500);        // delay needed for Wire.send to work reliably (really?)
		bitClear(I2C_FLAGS,I2C_TX_PACKET_READY);
		i2c_tx_length = 0;
		#if SERIAL_DEBUG > 0
			Serial.println("I2C Packet Sent");
		#endif
	}
	
}

void I2C_process_rx_packet(){

	uint8_t i2c_command;
	unsigned int crc = 0;  //store checksum for processing
	uint8_t cmd_length = 0;  //store end of current command
	uint8_t i2c_cmd_lengths[16] = {23,3,23,6,4,3,3,4,0,0,0,0,0,0,0,2};
	unsigned int cmd_crc = 0;
	uint8_t CRC_pass = 0;
	

	// Only process command if first nibble is 9 indicating payload command
	if((i2c_rxd_buffer[0]&0xF0) == 0x90)
	{
	
		i2c_command = i2c_rxd_buffer[0]&0x0F;
		cmd_length = i2c_cmd_lengths[i2c_command];
		
		// Pull the CRC from the message.
		crc = i2c_rxd_buffer[cmd_length]<<8;  // CRC is 2 bytes following cmd content
		crc = crc | i2c_rxd_buffer[cmd_length+1];
		
		// Do our own calculation of the message CRC
		cmd_crc = crc16(0xFFFF,i2c_rxd_buffer,cmd_length);
		
		if(crc == cmd_crc){ // Only proceed if CRCs are the same
			switch(i2c_command)
			{
			case 0: ;   pay_ops_initialise(cmd_crc);  // Payload Operations initialise 
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Ops Initialise");
						#endif
						break;
					
			case 1: ;   pay_ops_status();      // Payload Operations status
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Status Request");
						#endif
						break; 
					
			case 2: ;   pay_ops_update(cmd_crc);      // Payload Operations update
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Ops Update");
						#endif
						break;
			
			case 3: ;   pay_parameter_write(cmd_crc); // Payload Parameter write
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Paramater Write");
						#endif
						break;
			
			case 4: ;   pay_parameter_read();  // Payload Parameter read 
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Paramater Read");
						#endif
						break;
			
			case 5: ;   pay_pri_data_trans();  // Payload Priority Data Transfer 
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Priority Transfer");
						#endif
						break;
			
			case 6: ;   pay_data_trans();      // Payload Data Transfer 
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Data Txfer");
						#endif
						break;
			
			case 7: ;   //pay_SPI_data_trans();  // Payload SPI data transfer
						#if SERIAL_DEBUG > 0
							Serial.println("SPI Transfer - Unimplemented");
						#endif
						break;
			
			case 15: ;  pay_shutdn(cmd_crc);         // Payload Shutdown
						#if SERIAL_DEBUG > 0
							Serial.println("Payload Shutdown");
						#endif
						break;
						
			default:
						break;
			}
		}else{
			// CRC Failed. Send error packet.
			I2C_send_error(i2c_rxd_buffer[0], 0x01);
		}
	}
	
	I2C_clear_rx();
	bitClear(I2C_FLAGS,I2C_NEW_PACKET);

}

void I2C_send_crc_ack(unsigned int crc){
	i2c_txd_buffer[0] = (uint8_t)(crc>>8);
	i2c_txd_buffer[1] = (uint8_t)(crc&0xFF);
	I2C_send_packet(2);
}

void I2C_send_ack(uint8_t command){
	i2c_txd_buffer[0] = command;
	if(ERROR_FLAGS != 0){ // Something has raised an error flag. Send an error packet instead of an ACK.
		i2c_txd_buffer[1] = 0x09; // CRC bit is always set, and we have an error, hence 0b1001.
		i2c_txd_buffer[2] = ERROR_FLAGS;
	}else{ // Send the ACK
		i2c_txd_buffer[1] = 0x01;
		i2c_txd_buffer[2] = 0x7E;
	}
	unsigned int crc = crc16(0xFFFF,i2c_txd_buffer,3);
	i2c_txd_buffer[3] = (uint8_t)(crc>>8);
	i2c_txd_buffer[4] = (uint8_t)crc;
	I2C_send_packet(5); // Fire!
}

uint8_t I2C_wait_for_crc_ack(unsigned int crc){
	unsigned long start_time = millis();
	uint8_t got_ack = 0;
	while( ((millis() - start_time)<2000) && (got_ack == 0)){
		if(bitRead(I2C_FLAGS,I2C_NEW_PACKET)){
			if((i2c_rxd_buffer[0] == (crc>>8)) && (i2c_rxd_buffer[1] == (unsigned char)crc)){
				got_ack = 1;
				bitClear(I2C_FLAGS,I2C_NEW_PACKET);
			}
		}
	}
	return got_ack;
}

void I2C_send_error(uint8_t command, uint8_t error){
	i2c_txd_buffer[0] = command;
	i2c_txd_buffer[1] = 0x09;
	i2c_txd_buffer[2] = error;
	unsigned int crc = crc16(0xFFFF,i2c_txd_buffer,3);
	i2c_txd_buffer[3] = (uint8_t)(crc>>8);
	i2c_txd_buffer[4] = (uint8_t)crc;
	I2C_send_packet(5); // Fire!
}

uint8_t I2C_wait_for_ack(uint8_t command){
	unsigned long start_time = millis();
	uint8_t got_ack = 0;
	while( ((millis() - start_time)<2000) && (got_ack == 0)){
		if(bitRead(I2C_FLAGS,I2C_NEW_PACKET)){
			if(i2c_rxd_buffer[2] ==  0x7E && i2c_rxd_buffer[0] == command){ // We have an ACK packet.
				unsigned int crc = 0;
				crc = i2c_rxd_buffer[3]<<8;  // CRC is 2 bytes following cmd content
				crc = crc | i2c_rxd_buffer[4];
				unsigned int cmd_crc = crc16(0xFFFF,i2c_rxd_buffer,3);
				if(cmd_crc == crc){
					got_ack = 1;
				}
				bitClear(I2C_FLAGS,I2C_NEW_PACKET);
			}
			// Error packet detection here.
		}
	}
	return got_ack;
}

void I2C_send_packet(unsigned int len){
	i2c_tx_length = len;
	
	bitSet(I2C_FLAGS,I2C_TX_PACKET_READY);
	delay(1);
	while(bitRead(I2C_FLAGS,I2C_TX_PACKET_READY)){ 
		delayMicroseconds(50);
	}
}

// Clear I2C receive buffer
void I2C_clear_rx(void)
{
  int m;

  for(m = 0; m < (I2C_BUFSIZE); m++)
  {
    i2c_rxd_buffer[m] = 0x00;    // clear rxd_buffer[]
  }
}
/*----------------------------------------------------*/

/*----------------------------------------------------*/
// Clear I2C transmit buffer
void I2C_clear_tx(void)
{
  int m;

  for(m = 0; m < (I2C_BUFSIZE); m++)
  {
    i2c_txd_buffer[m] = 0x00;    // clear txd_buffer[]
  }
}